#include "gdt.h"
#include <string.h>
#include "dbgio.h"
void tss_initialize(int32_t num, uint16_t ss0, uint32_t esp0);
gdt_descriptor		x86_gdt[GDT_MAX_DESCRIPTORS];
gdt_pointer			x86_gdt_pointer;
extern "C" void tss_flush();
tss_entry_t tss_entry;

void load_gdt(){
	__asm {
		cli
		lgdt [x86_gdt_pointer]
	}
}

void gdt_setup_ringed(){
	tprintf("[0x%X] - _gdt_setup_ringed() \n",(void *)gdt_setup_ringed);
	x86_gdt_pointer.m_base = (uint32_t) x86_gdt;
	x86_gdt_pointer.m_limit =(GDT_MAX_DESCRIPTORS * sizeof(gdt_descriptor)) - 1;
	gdt_set_descriptor(0,0,0,0,0);
	gdt_set_descriptor(1,0,0xFFFFFFFF,GDT_PRESENT_BIT | GDT_READABLE_BIT | GDT_NON_SYSTEM_BIT
		| GDT_EXECUTABLE_BIT | GDT_SUPERVISOR_MASK,GDT_32_BIT_SEGMENT | GDT_GRANULARITY_4K_BIT);
	gdt_set_descriptor(2,0,0xFFFFFFFF,GDT_PRESENT_BIT | GDT_WRITABLE_BIT | GDT_NON_SYSTEM_BIT
		| GDT_SUPERVISOR_MASK,GDT_32_BIT_SEGMENT | GDT_GRANULARITY_4K_BIT);
	gdt_set_descriptor(3,0,0xFFFFFFFF,GDT_PRESENT_BIT | GDT_READABLE_BIT | GDT_NON_SYSTEM_BIT
		| GDT_EXECUTABLE_BIT | GDT_USERMODE_MASK,GDT_32_BIT_SEGMENT | GDT_GRANULARITY_4K_BIT);
	gdt_set_descriptor(4,0,0xFFFFFFFF,GDT_PRESENT_BIT | GDT_WRITABLE_BIT | GDT_NON_SYSTEM_BIT
		| GDT_USERMODE_MASK,GDT_32_BIT_SEGMENT | GDT_GRANULARITY_4K_BIT);
	tss_initialize(5,2 * GDT_SELECTOR_MULTIPLIER,0);
	load_gdt();
	tss_flush();
	tprintf(" gdt_setup_ringed: return\n");
}

void gdt_set_descriptor(uint32_t i, uint32_t base, uint32_t limit, uint8_t flags,uint8_t granularity){
	gdt_descriptor *gdt_d = &x86_gdt[i];
	if (i >= GDT_MAX_DESCRIPTORS)
		return;
	memset(gdt_d,0,sizeof(gdt_descriptor));
	gdt_d->baseLo		= (base >> GDT_BASE_LOW_SHIFT)	& GDT_BASE_LOW_MASK;
	gdt_d->baseMid		= (base >> GDT_BASE_MID_SHIFT)	& GDT_BASE_MID_MASK;
	gdt_d->baseHi		= (base >> GDT_BASE_HIGH_SHIFT)	& GDT_BASE_HIGH_MASK;
	gdt_d->limit		= (limit >> GDT_LIMIT_LOW_SHIFT)& GDT_LIMIT_LOW_MASK;
	gdt_d->granularity	= granularity | ((limit >> GDT_LIMIT_HIGH_SHIFT) & GDT_LIMIT_HIGH_MASK);
	gdt_d->flags		= flags;
}

void tss_initialize(int32_t num, uint16_t ss0, uint32_t esp0)
{
   uint32_t base = (uint32_t) &tss_entry;
   uint32_t limit = base + sizeof(tss_entry_t);
   gdt_set_descriptor(num, base, limit, 0xE9, 0x00);
   memset(&tss_entry, 0, sizeof(tss_entry_t));
   tss_entry.ss0  = ss0;
   tss_entry.esp0 = esp0;
   tss_entry.cs   = (1 * GDT_SELECTOR_MULTIPLIER) | RPL_USERMODE_MASK;
   tss_entry.ss = tss_entry.ds = tss_entry.es = tss_entry.fs = tss_entry.gs = (2 * GDT_SELECTOR_MULTIPLIER) | RPL_USERMODE_MASK;
} 

void tss_set_stack(uint32_t stack)
{
   tss_entry.esp0 = stack;
} 